-module(phys).
-compile(export_all).


go() ->
    go("127.0.0.1", 17676).


tele(X, Y, Dir) ->
    {X, Y, Dir}.

x(Tele) ->
    {X, _, _} = Tele,
    X.
y(Tele) ->
    {_, Y, _} = Tele,
    Y.
dir(Tele) ->
    {_, _, Dir} = Tele,
    Dir.


drive(Socket) ->

    Check = fun(_, _) -> not_reached end,
    Trans = fun(_, Tele) -> Tele end,
    Action = fun(_) -> gen_tcp:send(Socket, "a;") end,             

    {Check, Trans, Action, never_used}.



turn_to_home(Socket) ->
    Check = fun (_, {tele, NewT}) ->
		    Home = math:atan(y(NewT) / x(NewT)) * 180 / math:pi(),       % common state!!!

		    Delta = abs(dir(NewT) - Home),
		    if
			Delta < 4 ->
			    reached;
			true ->
			    not_reached
		    end 
	    end,		  

    Trans = fun (_M, Tele) -> Tele end,

    Action = fun ({tele, NewT}) ->
		     Home = math:atan(y(NewT) / x(NewT)) * 180 / math:pi(),      % common state!!! 		     
		     Rover = dir(NewT),
		     if 
			 Rover > Home ->
			     gen_tcp:send(Socket, "ar;");
			 true ->
			     gen_tcp:send(Socket, "al;")
		     end
	     end,

    Next = fun() -> drive(Socket) end,

    {Check, Trans, Action, Next}.



%% --------------------------------

initial() ->
    drive().


sender(S) ->
    receive
	Cmd when is_string(Cmd) ->
	    gen_tcp:send(S, Cmd)
    end.


start(Host, Port) ->
    {ok, Sock} = gen_tcp:connect(Host, Port, [list, {packet, 0}, {active, true}]),
    Send = spawn(fun() -> sender(Sock) end),
    register(sender, Send),

    Cont = spawn(fun() -> controller(void, initial()) end),
    register(cont, Cont),
    
    telemetry().



telemetry() ->
    receive
	{tcp, Sock, Bin} ->
	    recv(Bin, Rover),

	    telemetry()
    end.







controller(Socket, M, {Check, Trans, Action, Next}=G) ->                           % Socket is NOT USED actually!!!
    receive
	{tele, _}=Tele ->

	    case Check(M, Tele) of
		reached ->
		    {NewCheck, NewTrans, NewAction, NewNext} = Next(),
		    NewAction(M),

		    controller(Socket, 
			       NewTrans(M), {NewCheck, NewTrans, NewAction, NewNext});

		not_reached ->
		    
		    Action(M),
		    controller(Socket,
			       Trans(M), G)
	    end;

	_ ->
	    io:format("Controller is alive. Socket=~p~n", [Socket])              % only here!!!

%%     after 80 ->
%% 	    Action(M)

    end.
    
%% {_, Time1} = statistics(runtime),
%% {_, Time2} = statistics(wall_clock),
%% U1 = Time1 * 1000,
%% U2 = Time2 * 1000,
%% io:format("Time=~p (~p) microsec~n", [U1, U2]),



%% TODO monad???
client(_, [{count, 0}]) ->
    ok;

client({Socket, _}=Rover, Opt) ->


recv([H|T], {_, Pid}) ->               % !!! Pid only
    [Data|_] = string:tokens(T, ";"),
    case H of
	$I ->
	    init(Data);
	$T ->
	    telemetry(Data, Pid)
%
%	    gen_tcp:send(Socket, "a;")	    
%    
    end.


telemetry(Data, Pid) -> 
    [H|T] = string:tokens(Data, " "),
    [TimeStamp] = read([H]),
    [Ctl|T10] = T,
    {[X, Y, Dir, Speed], Rest} = read(T10, 4),

    Pid ! {tele, tele(X, Y, Dir)}.

%    io:format("rest = ~p~n", [Rest]),
%% rest = ["m","170.052","-144.553","-172.2","1.480","b","121.094",
%%         "-105.469","0.726","b","121.094","-121.094","2.024"]
    
%    io:format("received telemetry = ~p ~p ~p ~p ~p ~p~n", [TimeStamp, Ctl, X, Y, Dir, Speed]).



%% one telemetry function???
init(H) ->
    [Dx, Dy, TimeLimit, MinSensor, MaxSensor, MaxSpeed, MaxTurn, MaxHardTurn] = read(string:tokens(H, " ")).
%    io:format("received initialization = ~p ~p ~p ~p ~p ~p ~p ~p~n", [Dx, Dy, TimeLimit, MinSensor, MaxSensor, MaxSpeed, MaxTurn, MaxHardTurn]).
%    Pid ! {ini, }.

readf(X) ->
%%          fun(X) -> 
	      case io_lib:fread("~f", X) of 
		  {ok, [Y], _} -> 
		      Y; 
		  {error, _} -> 
		      {ok, [Y], _} = io_lib:fread("~d", X), 
		      Y 
	      end.

read(L) ->
    lists:map(fun(X) -> readf(X) end, L).



read(L, Count) ->
    read(L, Count, []).

read(L, 0, R) ->
    {lists:reverse(R), L};
read([H|T], Count, R) ->
    read(T, Count - 1, [readf(H)|R]).

